#include <user.h>
#include <io.h>
#include <ts7200.h>
#include <curses.h>
#include <string.h>
#include <defs.h>
#include <bwio.h>
#include <controller.h>
#include <printer.h>
#include <track.h>
#include <printer.h>
#include <sensor.h>
#include <utils.h>
#include <engine.h>
#include <messages.h>
#include <pathplanner.h>

void NameServer();
void ClockServer();
void Controller();
void Printer();
void SensorManager();
void TrackManager();
void TerminalTxServer();
void TerminalRxServer();
void TrainTxServer();
void TrainRxServer();
void IdleTask();
void EngineManager();
void RouteFinder();
void PathPlanner();

// buffer sizes
#define CMD_BUF_SIZE 16

// command constants
#define TRAIN_SOLENOID_OFF  0x20
#define TRAIN_SW_STRAIGHT   0x21
#define TRAIN_SW_CURVED     0x22
#define TRAIN_REVERSE       0x0f
#define TRAIN_GO            0x60
#define TRAIN_STOP          0x61
#define TRAIN_SENSORS_BASE  0x80
#define TRAIN_SENSOR_BASE   0xc0

// message types
#define CLOCK_MSG       0
#define SENSOR_MSG      1
#define USER_INPUT_MSG  2

// interface message
typedef struct __interface_msg {
  int type;
  int c;
  int dc_ticks;
  char ss_status[10];
} interface_msg;

void UpdateDisplayClock(int dc_ticks) {
  // SaveCursor();
  PrintAt(1, 1, "%d%d:%d%d.%d", dc_ticks/6000, (dc_ticks/600)%10,
      (dc_ticks/100)%6, (dc_ticks/10)%10, dc_ticks%10);
  // LoadCursor();
}

void UpdateSwitchDisplay(int switch_num, char switch_dir) {
  // SaveCursor();
  if (switch_num < 12) {
    PrintAt(1, switch_num + 6, "%d: %c   ", switch_num, switch_dir);
  }
  else if (switch_num < 19) {
    PrintAt(8, switch_num - 5, "%d: %c   ", switch_num, switch_dir);
  }
  else if (switch_num >= 153 && switch_num < 157) {
    PrintAt(8, switch_num - 153 + 19 - 5, "%x: %c   ", switch_num, switch_dir);
  }
  // LoadCursor();
}

void SwitchDisplay() {
  // SaveCursor();
  PrintAt(1, 4, "State of turn outs\r\n------------------");
  
  int i, j;
  for (i = 1; i < 12; i++) {
    PrintAt(1, i + 6, "%d: %c   ", i, GetSwitchState(i));
  }
  for (j = 7; i < 19; i++, j++) {
    PrintAt(8, j, "%d: %c   ", i, GetSwitchState(i));
  }
  for (i = 153; i < 157; j++, i++) {
    PrintAt(8, j, "%x: %c   ", i, GetSwitchState(i));
  }
  // LoadCursor();
}

void ClockMonitor() {
  int dc_ticks = 0;

  RegisterAs("clock-monitor");

  int interface_tid;
  Receive(&interface_tid, NULL, 0);
  Reply(interface_tid, NULL, 0);
  for ( ; ; ) {
    UpdateDisplayClock(dc_ticks);
    AwaitEvent(EVENT_TIMER, NULL, 0);
    dc_ticks = Time()/2;
  }
  Panic("ClockMonitor", "Chuck Norris!");
}

void SensorMonitor() {
  char name[5];
  char sensor_data[10];

  RegisterAs("sensor-monitor");

  int interface_tid;
  Receive(&interface_tid, NULL, 0);
  Reply(interface_tid, NULL, 0);

  for ( ; ; ) {
    AwaitEvent(EVENT_NEW_SENSOR, sensor_data, 10);
    SensorToExternal(sensor_data, name);
    PrintAt(0, 2, "%s    ", name);
  }
  Panic("SensorMonitor", "Chuck Norris!");
}

void UserInputMonitor() {
  interface_msg msg;
  msg.type = USER_INPUT_MSG;

  int interface_tid;
  Receive(&interface_tid, NULL, 0);
  Reply(interface_tid, NULL, 0);

  for ( ; ; ) {
    msg.c = Getc(COM2);
    Send(interface_tid, (char*)&msg, sizeof(interface_msg), NULL, 0);
  }
}

int ParseInput(char *buf, char **cmd, char **a1, char **a2) {
  char *pos;
  *cmd = StringToken(buf, &pos, ' ');
  if (*cmd == NULL) return 0;
  *a1 = StringToken(NULL, &pos, ' ');
  if (*a1 == NULL) return 1;
  *a2 = StringToken(NULL, &pos, ' ');
  if (*a2 == NULL) return 2;
  return 3;
}

int DoCommand(char *cmd_buf) {
  char *cmd, *a1, *a2;
  int argc, v1, v2;
  argc = ParseInput(cmd_buf, &cmd, &a1, &a2);
  if (StringCompare(cmd, "tr", 2)) {
    if (argc != 3) {
      PrintAt(1, 24, "Usage: tr train_number train_speed\r\n" );
      return 1;
    }
    v1 = Atoi(a1); v2 = Atoi(a2);
    if (v1 < 1 || v1 > 80) {
      PrintAt(1, 24, "Error: invalid train number %d!\r\n", v1 );
      return 1;
    }
    if (v2 < 0 || v2 > 14) {
      PrintAt(1, 24, "Error: invalid train speed %d!\r\n", v2 );
      return 1;
    }
    Tr(v1, v2);
  } else if (StringCompare(cmd, "p", 1)) {
    Panic("PanicCommand", "User-induced panic.");
  } else if (StringCompare(cmd, "rv", 2)) {
    if (argc != 2) {
      PrintAt(1, 24, "got %d args, expected 2\r\n", argc );
      PrintAt(1, 24, "Usage: rv train_number\r\n" );
      return 1;
    }
    v1 = Atoi(a1);
    if (v1 < 1 || v1 > 80) {
      PrintAt(1, 24, "Error: invalid train number %d!\r\n", v1 );
      return 1;
    }
    Rv(v1);
  } else if (StringCompare(cmd, "sw", 2)) {
    if (argc != 3) {
      PrintAt(1, 24, "Usage: sw switch_number switch_direction\r\n" );
      return 1;
    }
    v1 = Atoi(a1);
    if (v1 < 0 || v1 > 255) {
      PrintAt(1, 24, "Error: invalid switch number %d!\r\n", v1 );
      return 1;
    }
    if (a2[0] != 'S' && a2[0] != 'C') {
      PrintAt(1, 24, "Error: invalid switch direction %c!\r\n", a2[0] );
      return 1;
    }
    Sw(v1, a2[0]);
    UpdateSwitchDisplay(v1, a2[0]);
  } else if (StringCompare(cmd, "goto", 4)) {
    if (argc != 3) {
      PrintAt(0, 24, "got %d args, expected 3\r\n", argc );
      PrintAt(0, 24, "Usage: goto train_number landmark\r\n" );
      return 1;
    }
    v1 = Atoi(a1);
    if (v1 < 1 || v1 > 80) {
      PrintAt(0, 24, "Error: invalid train number %d!\r\n", v1 );
      return 1;
    }
    v2 = ExternalToInternal(a2);
    // TODO: error check v2!
    // int path[72];
    //FindRoute(66, v2, path);
    SendTrainTo(v1, WhereAmI(), v2);
  } else if (StringCompare(cmd, "stop", 4)) {
    Stop();
  } else if (StringCompare(cmd, "go", 2)) {
    Go();
  } else if (StringCompare(cmd, "q", 1)) {
    PrintAt(1, 24, "Exiting kernel...\r\n");
    FingerOfDeath();
  } else {
    PrintAt(1, 24, "%s: command not found\r\n", cmd);
    return 1;
  }
  return SUCCESS;
}

void InterfaceInit() {
  int i;
  
  // initialize screen
  ResetScreen();
  ClearScreen();
  SetScreenColor(FG_BASE + WHITE_OFFSET, BG_BASE + BLACK_OFFSET);
  PrintAt(1, 1, ""
"                                                                           \r\n"
"    _/_/_/_/                                _/          _/_/      _/_/_/   \r\n" 
"   _/    _/    _/_/_/  _/    _/    _/_/_/  _/        _/    _/  _/          \r\n"
"  _/_/_/    _/_/      _/    _/  _/        _/_/_/    _/    _/    _/_/       \r\n");
PrintAt(1, 5, ""
" _/            _/_/  _/    _/  _/        _/    _/  _/    _/        _/      \r\n"
"_/        _/_/_/      _/_/_/    _/_/_/  _/    _/    _/_/    _/_/_/         \r\n"
"                        _/                                                 \r\n"
"                   _/_/                                                    \r\n");


  PrintAt(1, 13, "From PsychOS to psychos...");
  PrintAt(1, 15, "Initializing...");
  PrintAt(1, 16, "Stopping trains...");

  Tr(9, 0);
  Tr(24, 0);
  Tr(25, 0);
  Tr(46, 0);
  Tr(52, 0);
  Tr(55, 0);
  
  Delay(20);
  PrintAt(1, 17, "Setting switches...");
  for (i = 1; i <= 156; i++) {
    if (i == 19) i = 153;   // skip non-existent switches
    Sw(i, 'C');
  }
  
  PrintAt(1, 19, "Done.\r\n\r\nPlease enter any key to continue..." );
  Getc(COM2);
  
  
  ClearScreen();  
  SetScrollableRegion(20, 24);
  PrintAt(1, 24, "> ");
  
  Send(WhoIs("clock-monitor"), NULL, 0, NULL, 0);
  Send(WhoIs("sensor-monitor"), NULL, 0, NULL, 0);
  
  SwitchDisplay();
}

void Interface() {

  // initialize command-line buffers
  char cmd_buf[CMD_BUF_SIZE+1];
  cmd_buf[0] = '\0';
  int cmd_ptr = 0;

  // initialize space for messages
  interface_msg msg;
  
  InterfaceInit();
  
  int user_input_tid = Create(5, &UserInputMonitor, "UserInputMonitor");
  Send(user_input_tid, NULL, 0, NULL, 0);

  int sender_tid;
  for ( ; ; ) {
    Receive(&sender_tid, (char*)&msg, sizeof(interface_msg));
    if (sender_tid == user_input_tid) {
      if (msg.c == '\r') {
        PrintAt(3 + cmd_ptr, 24, "\r\n" );
        cmd_buf[cmd_ptr] = '\0';
        if (cmd_ptr != 0)
          DoCommand(cmd_buf);
        PrintAt(1, 24, "> " );
        cmd_ptr = 0;
      } else if (msg.c == '\x7f' && cmd_ptr > 0) {
        cmd_ptr--;
        PrintAt(cmd_ptr + 3, 24, "%c", ' ');
      } else if (IsAlnum(msg.c) && cmd_ptr < CMD_BUF_SIZE) {
        PrintAt(cmd_ptr + 3, 24, "%c", msg.c);
        cmd_buf[cmd_ptr++] = msg.c;
      }
      Reply(user_input_tid, NULL, 0);
    }
  }
  Exit();
}

void UserInit() {
  int child;
  child = Create( 6, &NameServer, "NameServer" );
  Send(child, NULL, 0, NULL, 0);
  child = Create( 9, &ClockServer, "ClockServer" );
  Send(child, NULL, 0, NULL, 0);  
  child = Create( 8, &Printer, "Printer" );
  Send(child, NULL, 0, NULL, 0);  
  child = Create( 9, &TerminalTxServer, "TerminalTxServer" );
  Send(child, NULL, 0, NULL, 0);
  child = Create( 6, &TerminalRxServer, "TerminalRxServer" );
  Send(child, NULL, 0, NULL, 0);
  child = Create( 8, &TrainTxServer, "TrainTxServer" );
  Send(child, NULL, 0, NULL, 0);
  child = Create( 9, &TrainRxServer, "TrainRxServer" );
  Send(child, NULL, 0, NULL, 0);  
  //child = Create( 4, &SwitchManager, "SwitchManager" );
  //Send(child, NULL, 0, NULL, 0);
  child = Create( 8, &Controller, "Controller" );
  Send(child, NULL, 0, NULL, 0);
  
  Create(0, &IdleTask, "IdleTask" );
  
  ResetScreen();
  ClearScreen();
  PrintAt(1, 19, "Please enter the track type (A or B): " );
  char c;  
  do {
    c = Getc(COM2);
  }
  while (c != 'A' && c != 'B');
  char track = c;
  PrintAt(40, 19, "%c", c);
  
  PrintAt(1, 20, "Geben Sie bitte die Zugzahl ein: " );
  int train_number = 0;
  int i = 0;
  while (TRUE) {
    c = Getc(COM2);
    if (c == '\r')
      break;
    if (c >= '0' && c <= '9') {
      PrintAt(34 + i++, 20, "%c", c);
      train_number = train_number * 10 + (c - '0');
    }
  }
  
  child = Create( 3, &TrackManager, "TrackManager" );
  Send(child, &track, 1, NULL, 0);
  child = Create( 9, &SensorManager, "SensorManager");
  Send(child, &track, 1, NULL, 0);
  child = Create( 7, &EngineManager, "EngineManager");
  Send(child, (char*)&train_number, sizeof(int), NULL, 0);
  child = Create( 4, &RouteFinder, "RouteFinder");
  Send(child, &track, 1, NULL, 0);
  child = Create( 9, &PathPlanner, "PathPlanner");
  Send(child, NULL, 0, NULL, 0);
  
  Create(5, &ClockMonitor, "ClockMonitor");
  Create(5, &SensorMonitor, "SensorMonitor");
  Create(7, &Interface, "Interface" );
  Exit();
}
